/**
 * @file lateral_control
 * @author yq
 * @brief 
 * @version 0.1
 * @date 24-3-18
 *
 * @copyright Copyright (c) 2024
 */
#include "lateral_control.h"


LQR::LQR(Eigen::Matrix <double, 3, 3> Q, Eigen::Matrix <double, 2, 2> R,
         double eps, int maxIter, VehicleConfig vehicle_config) {
    this->Q_ = Q;
    this->R_ = R;
    this->eps = eps;
    this->maxIteration_ = maxIter;
    error_x_ = 0;
    error_y_ = 0;
    error_theta_ = 0;
    l = vehicle_config.l;
    steer_ratio = vehicle_config.steer_ratio;
    first_run_ = true;

    // 获取当前系统时间
    auto current_time = std::chrono::system_clock::now();
    std::time_t time_t_date = std::chrono::system_clock::to_time_t(current_time);

    // 将时间格式化为字符串
//    std::tm tm_date = *std::localtime(&time_t_date);
//    std::stringstream ss;
//    ss << std::put_time(&tm_date, "/home/yq/log/lqr_log%Y-%m-%d_%H-%M-%S.csv");
//    std::string filename = ss.str();
//
//    outFile = std::ofstream(filename, std::ios_base::app); ; // 创建一个指向output.txt的输出文件流对象
//    outFile << "error_x_" << "," << "error_y_" << ","<< "error_theta_" << ","<<
//            "trace_theta_" << "," <<  "trace_delta_" << "," << "trace_kappa_" << "," << "steer" << std::endl; // 写入文件
}


void LQR::LatExecutiveControl(const perception_msgs::TrajectoryPoint trace_point,
                              const CarState& vehicle_state,
                              ControlData& control_data) {
    // 控制器输入
    error_x_ =  vehicle_state.x - trace_point.position.x;
    error_y_ =  vehicle_state.y - trace_point.position.y;
    ROS_INFO("yaw: %f, target_yaw: %f", degreesToRadians(vehicle_state.yaw), normalizeAngle0To2Pi(trace_point.heading));
    error_theta_ = normalizeAngleError(degreesToRadians(vehicle_state.yaw) - normalizeAngle0To2Pi(trace_point.heading));
    trace_kappa_ = trace_point.curvature;
    trace_delta_ = atan(l * trace_kappa_);
    trace_theta_ = trace_point.heading;
    currentTime_ = std::chrono::steady_clock::now();
    if (first_run_) {
        dt_ = 1.0 / 80.0; // 控制频率100hz
        first_run_ = false;
    } else {
        std::chrono::duration<double> duration = currentTime_ - lastTime_;
        dt_ = duration.count();
        dt_ = 1.0 / 80.0;
    }
    // 运动学方程矩阵A、B
    Eigen::Matrix<double, 3, 3> A;
    double A02 = -dt_ * vehicle_state.v * sin(trace_theta_);
    double A12 = dt_ * vehicle_state.v * cos(trace_theta_);
    A << 1, 0, A02,
         0, 1, A12,
         0, 0, 1;
    Eigen::Matrix<double, 3, 2> B;
    double B00 = dt_ * cos(trace_theta_);
    double B10 = dt_ * sin(trace_theta_);
    double B20 = dt_ * tan(trace_delta_) / l;
    double B21 = dt_ * vehicle_state.v / (l * pow(cos(trace_delta_), 2));
    B << B00, 0,
         B10, 0,
         B20, B21;
    is_solve_ = false;

    // 求解LQR黎卡提方程
    Eigen::Matrix<double, 2, 3> K = Solve(A, B, this->Q_, this->R_, is_solve_);
    if (!is_solve_) {
        control_data.steer = 0;
        ROS_INFO("LQR slove failed!!!");
        ROS_INFO("-------------------LQR------------------");
        ROS_INFO("error_x_: %f", error_x_);
        ROS_INFO("error_y_: %f", error_y_);
        ROS_INFO("error_theta_: %f", error_theta_);
        ROS_INFO("trace_kappa_: %f", trace_kappa_);
        ROS_INFO("trace_delta_: %f", trace_delta_);
        ROS_INFO("trace_theta_: %f", trace_theta_);
        ROS_INFO("dt_: %f", dt_);
        ROS_INFO("is_solve_: %s", is_solve_ ? "true" : "false");
        ROS_INFO("steer: %f", control_data.steer);
        ROS_INFO("-----------------------------------------");
        return;
    }

    // 计算反馈控制量
    Eigen::Matrix<double, 3, 1> X;
    X << this->error_x_,
         this->error_y_,
         this->error_theta_;

    double steer = std::clamp(radiansToDegrees(-(K * X)(1, 0)) * steer_ratio, -540.0, 540.0);
    control_data.steer = steer;

//    if (outFile.is_open()) {
//        outFile << std::fixed << std::setprecision(6) << error_x_ << "," << error_y_ << ","<< error_theta_ << ","<<
//        trace_theta_ << "," <<  trace_delta_ << "," << trace_kappa_ << "," << steer
//        << "," << trace_point.position.y << "," << trace_point.position.y << "," << trace_point.heading << std::endl; // 写入文件
//        std::cout << std::fixed << std::setprecision(6) << error_x_ << ", " << error_y_ << ", "<< error_theta_ << ", "<<
//        trace_theta_ << ", " <<  trace_delta_ << ", " << trace_kappa_ << ", " << steer << ", " << trace_point.position.y << ", " << trace_point.position.y << ", " << trace_point.heading << std::endl; // 写入文件
//    } else {
//        std::cerr << "Unable to open file for writing." << std::endl; // 如果文件无法打开，输出错误
//    }

//    // 方向盘转角约束
//    double dsteerMax = this->dt_ * vehicle_config.max_steer_angle_rate;
//    double dsteerMin = this->dt_ * vehicle_config.min_steer_angle_rate;
//    double dsteer = LQR_output - control_data.steer;

    ROS_INFO("-------------------LQR------------------");
    ROS_INFO("error_x_: %f", error_x_);
    ROS_INFO("error_y_: %f", error_y_);
    ROS_INFO("error_theta_: %f", error_theta_);
    ROS_INFO("trace_kappa_: %f", trace_kappa_);
    ROS_INFO("trace_delta_: %f", trace_delta_);
    ROS_INFO("trace_theta_: %f", trace_theta_);
    ROS_INFO("dt_: %f", dt_);
    ROS_INFO("is_solve_: %s", is_solve_ ? "true" : "false");
    ROS_INFO("steer: %f", steer);
    ROS_INFO("-----------------------------------------");

    lastTime_ = std::chrono::steady_clock::now();
}

//Eigen::MatrixXd LQR::Solve(Eigen::Matrix<double,3,3> A, Eigen::Matrix<double, 3, 2>  B,
//                           Eigen::Matrix<double, 3, 3>  Q, Eigen::Matrix<double, 2, 2>  R, bool& is_solve) {
//    Eigen::Matrix<double, 3, 3> X = Q;
//    Eigen::Matrix<double, 3, 3> Xn;
//    for (size_t i = 0; i < maxIteration_; ++i) {
//        Eigen::MatrixXd XB = R + B.transpose() * X * B;
//        Xn = A.transpose() * X * A - A.transpose() * X * B *
//                                     XB.inverse() * B.transpose() * X * A + Q;
//        Eigen::Matrix<double, 3, 3> XR = Xn - X;
//        double max = 0;
//        for (size_t j = 0; j < 3; ++j) {
//            for (size_t k = 0; k < 3; ++k)
//                max = abs(XR(j,k)) > max ? XR(j, k)  : max;
//        }
//        std::cout << "max:" << max << std::endl;
//        if (max < eps) {
//            is_solve = true;
//            break;
//        }
//        X = Xn;
//    }
//    Eigen::MatrixXd XB = R + B.transpose() * Xn * B;
//    Eigen::MatrixXd K = XB.inverse() * B.transpose() * Xn * A;
//    return K;
//}

// 函数用于解决离散时间代数Riccati方程（DARE）
Eigen::MatrixXd LQR::Solve(Eigen::Matrix<double,3,3> A, Eigen::Matrix<double, 3, 2>  B,
                           Eigen::Matrix<double, 3, 3>  Q, Eigen::Matrix<double, 2, 2>  R, bool& is_solve) {

    Eigen::Matrix<double, 3, 3> X = Q; // 初始化X
    Eigen::Matrix<double, 3, 3> Xn;

    for (int i = 0; i < maxIteration_; ++i) {
        // 更新 Xn 的计算
        Eigen::Matrix<double, 2, 2> XB = R + B.transpose() * X * B;
        Xn = A.transpose() * X * A - A.transpose() * X * B * XB.inverse() * B.transpose() * X * A + Q;

        // 计算误差的最大绝对值
        Eigen::Matrix<double, 3, 3> XR = Xn - X;
        double max_error = 0;
        for (int j = 0; j < 3; ++j) {
            for (int k = 0; k < 3; ++k) {
                max_error = std::max(max_error, std::abs(XR(j, k)));
            }
        }
//        std::cout << "max_error: " << max_error << std::endl;

        // 判断是否收敛
        if (max_error < eps) {
            is_solve = true;
            break;
        }
        X = Xn; // 更新 X 的值
    }

    // 计算K
    Eigen::MatrixXd XB = R + B.transpose() * Xn * B;
    Eigen::MatrixXd K = XB.inverse() * B.transpose() * Xn * A;
    return K;
}